import numpy as np

'''
原生库计算气动力矩（机体轴）
INPUT: 无风电压, 有风电压, 迭代轮数
OUTPUT: 机体轴气动力矩, 动压
'''
def calc_f(u0_temp, u1_temp, iter_num):
    # 静校系数矩阵
    matrix_steady = [
    [4.3103E-02, 1.8165E-04, 1.9002E-04, 1.7390E-02, -1.3530E-02, -2.7979E-04], 
    [1.8821E-01, 1.8495E-03, -1.5446E-04, 2.6557E-02, -1.3018E-02, 1.1612E-02], 
    [1.7915E-01, 2.5862E-02, 1.7016E-03, -2.8987E-01, -1.5251E-01, 1.3021E-03], 
    [4.4462E-04, 2.6741E-03, 1.7876E-05, 4.4869E-02, 2.8351E-05, -1.5571E-04], 
    [5.7609E-03, 4.0712E-05, 2.5701E-03, -4.3074E-03, 3.9853E-02, 2.2058E-04], 
    [1.0680E-01, -8.2055E-04, 9.5435E-03, 1.5814E-01, 2.5754E-02, 1.7124E-03], 
    [2.3402E-06, 1.4493E-08, 5.3106E-07, 6.9791E-06, 2.7474E-07, 1.2749E-09], 
    [-8.5185E-05, -2.4816E-05, 2.6132E-05, -2.2706E-04, -1.9607E-04, 1.6430E-04], 
    [-1.1741E-03, 1.3368E-06, 7.3650E-05, -5.3986E-03, 5.2172E-04, 2.4185E-04], 
    [3.3077E-06, 3.1378E-08, 1.6392E-07, 4.6726E-06, -6.6846E-07, 2.1713E-07], 
    [9.0448E-07, 3.4180E-07, -7.0419E-08, 2.1698E-05, -7.9508E-06, 3.2583E-06], 
    [6.8471E-04, 3.6699E-05, 1.6259E-05, 6.5897E-04, 1.9410E-04, 4.6028E-05], 
    [1.0087E-05, 7.6442E-07, 1.3526E-07, -1.3611E-04, 2.1069E-06, 2.2067E-06], 
    [5.2161E-05, -4.9483E-06, -3.9964E-06, 1.4572E-05, -5.8234E-04, -3.7876E-06], 
    [-2.0322E-06, -2.3312E-07, 1.2011E-08, 1.5021E-05, 1.1146E-07, -8.9502E-07], 
    [1.4338E-07, -1.4738E-08, -1.7175E-07, -2.4646E-07, 2.5512E-06, -8.8130E-08], 
    [1.2337E-05, 6.1090E-07, -1.5980E-06, 2.8014E-05, 3.2890E-05, -1.1704E-05], 
    [7.8037E-04, 4.5488E-08, 9.4849E-05, -5.3573E-04, -5.7408E-03, 3.4022E-04], 
    [2.0061E-04, 7.7029E-06, -3.8332E-07, 2.4403E-06, 2.8631E-06, -9.6610E-07], 
    [-7.0329E-05, 2.5158E-06, -3.2003E-06, 1.1711E-05, 2.1044E-06, -2.7340E-07], 
    [4.4171E-04, 5.6192E-06, -6.5533E-05, 4.5953E-04, 7.8849E-04, -4.9653E-05], 
    [1.2832E-04, -8.9651E-07, -1.0036E-05, -1.1550E-04, 1.6642E-05, 2.4827E-05], 
    [5.6780E-04, 2.5590E-05, 1.4705E-05, 1.8901E-05, 7.1230E-06, -6.6535E-07], 
    [-4.5412E-03, -3.7130E-04, -1.2242E-04, 8.2603E-03, -1.2933E-04, 2.9391E-04], 
    [3.1990E-06, 3.7609E-07, -2.6279E-09, 2.4632E-06, 3.7303E-06, 8.4015E-07], 
    [1.3725E-05, 1.0248E-06, -2.6190E-06, -3.4757E-05, -1.3039E-04, 5.6598E-05], 
    [4.5639E-05, -1.8107E-06, -3.6958E-06, 1.0346E-04, 6.8301E-05, 7.1515E-06], 
    ]
    
    # 天平电压
    # X, Y, Mz, Mx, My Z, q
    u0 = [0]*7
    u1 = [0]*7

    u0 = u0_temp
    u1 = u1_temp


    # 计算气动力电压
    # X, Y, Mz, Mx, My Z, q
    # Y, Mz, Mx, X, Z, My
    def change_order(ua):
        ub = [0]*6
        ub[0] = ua[2] - ua[1] # Y
        ub[1] = ua[1] + ua[2] # Mz
        ub[2] = ua[3] # Mx
        ub[3] = ua[0] # X
        ub[4] = ua[5] - ua[4] # Z
        ub[5] = ua[4] + ua[5] # My
        return ub

    # 动压电压
    uq0 = u0[-1]
    uq1 = u1[-1]

    # 计算动压
    q = (uq1-uq0)/24.26097143*9.80665

    # 气动力电压
    # Y, Mz, Mx, X, Z, My
    uf0 = change_order(u0)
    uf1 = change_order(u1)
    ufd = [0]*len(uf0)

    for i in range(len(ufd)):
        ufd[i] = uf1[i] - uf0[i]

    # 计算初始气动力
    f0 = [1]*6
    f = [1]*6


    for i in range(6):
        f0[i] = ufd[i] * matrix_steady[i][i]

    # 气动力迭代
    # Y, Mz, Mx, X, Z, My
    def iter_force(f1, f0):
        X = f1[3]
        Y = f1[0]
        Z = f1[4]
        Mx = f1[2]
        My = f1[5]
        Mz = f1[1]

        item = [0]*27

        item = [Y, Mz, Mx ,X, Z, My,
                Y*Y, Mz*Mz, Mx*Mx ,X*X, Z*Z, My*My,
                Y*Mz, Y*Mx ,Y*X, Y*Z, Y*My,
                Mz*Mx, Mz*X, Mz*Z, Mz*My,
                Mx*X, Mx*Z, Mx*My,
                X*Z, X*My,
                Z*My]

        f2 = [0]*6


        for i in range(6):
            coeff = [0]*27
            for j in range(27):
                coeff[j] = matrix_steady[j][i]
            coeff[i] = 0
            
            temp = 0
            
            for j in range(27):
                temp = temp + coeff[i]*item[i]

            f2[i] = f0[i] + temp

        return f2

    if iter_num == 0:
        f = f0
    else:
        for i in range(iter_num):
            f = iter_force(f, f0)

    # 转换顺序
    # Y, Mz, Mx, X, Z, My
    # X, Y, Z, Mx, My, Mz, Q
    ft = [0]*6
    ft[0] = f[3]
    ft[1] = f[0]
    ft[2] = f[4]
    ft[3] = f[2]
    ft[4] = f[5]
    ft[5] = f[1]

    return ft, q
                

'''
Numpy库计算气动力矩（机体轴）
INPUT: 无风电压, 有风电压, 迭代轮数
OUTPUT: 机体轴气动力矩, 动压
'''
def calc_f2(u0, u1, iter_num):
    # 静校系数矩阵
    matrix_steady = [
    [4.3103E-02, 1.8165E-04, 1.9002E-04, 1.7390E-02, -1.3530E-02, -2.7979E-04],
    [1.8821E-01, 1.8495E-03, -1.5446E-04, 2.6557E-02, -1.3018E-02, 1.1612E-02],
    [1.7915E-01, 2.5862E-02, 1.7016E-03, -2.8987E-01, -1.5251E-01, 1.3021E-03],
    [4.4462E-04, 2.6741E-03, 1.7876E-05, 4.4869E-02, 2.8351E-05, -1.5571E-04],
    [5.7609E-03, 4.0712E-05, 2.5701E-03, -4.3074E-03, 3.9853E-02, 2.2058E-04],
    [1.0680E-01, -8.2055E-04, 9.5435E-03, 1.5814E-01, 2.5754E-02, 1.7124E-03],
    [2.3402E-06, 1.4493E-08, 5.3106E-07, 6.9791E-06, 2.7474E-07, 1.2749E-09],
    [-8.5185E-05, -2.4816E-05, 2.6132E-05, -2.2706E-04, -1.9607E-04, 1.6430E-04],
    [-1.1741E-03, 1.3368E-06, 7.3650E-05, -5.3986E-03, 5.2172E-04, 2.4185E-04],
    [3.3077E-06, 3.1378E-08, 1.6392E-07, 4.6726E-06, -6.6846E-07, 2.1713E-07],
    [9.0448E-07, 3.4180E-07, -7.0419E-08, 2.1698E-05, -7.9508E-06, 3.2583E-06],
    [6.8471E-04, 3.6699E-05, 1.6259E-05, 6.5897E-04, 1.9410E-04, 4.6028E-05],
    [1.0087E-05, 7.6442E-07, 1.3526E-07, -1.3611E-04, 2.1069E-06, 2.2067E-06],
    [5.2161E-05, -4.9483E-06, -3.9964E-06, 1.4572E-05, -5.8234E-04, -3.7876E-06],
    [-2.0322E-06, -2.3312E-07, 1.2011E-08, 1.5021E-05, 1.1146E-07, -8.9502E-07],
    [1.4338E-07, -1.4738E-08, -1.7175E-07, -2.4646E-07, 2.5512E-06, -8.8130E-08],
    [1.2337E-05, 6.1090E-07, -1.5980E-06, 2.8014E-05, 3.2890E-05, -1.1704E-05],
    [7.8037E-04, 4.5488E-08, 9.4849E-05, -5.3573E-04, -5.7408E-03, 3.4022E-04],
    [2.0061E-04, 7.7029E-06, -3.8332E-07, 2.4403E-06, 2.8631E-06, -9.6610E-07],
    [-7.0329E-05, 2.5158E-06, -3.2003E-06, 1.1711E-05, 2.1044E-06, -2.7340E-07],
    [4.4171E-04, 5.6192E-06, -6.5533E-05, 4.5953E-04, 7.8849E-04, -4.9653E-05],
    [1.2832E-04, -8.9651E-07, -1.0036E-05, -1.1550E-04, 1.6642E-05, 2.4827E-05],
    [5.6780E-04, 2.5590E-05, 1.4705E-05, 1.8901E-05, 7.1230E-06, -6.6535E-07],
    [-4.5412E-03, -3.7130E-04, -1.2242E-04, 8.2603E-03, -1.2933E-04, 2.9391E-04],
    [3.1990E-06, 3.7609E-07, -2.6279E-09, 2.4632E-06, 3.7303E-06, 8.4015E-07],
    [1.3725E-05, 1.0248E-06, -2.6190E-06, -3.4757E-05, -1.3039E-04, 5.6598E-05],
    [4.5639E-05, -1.8107E-06, -3.6958E-06, 1.0346E-04, 6.8301E-05, 7.1515E-06]]

    matrix_steady = np.array(matrix_steady)

    # 天平电压
    # X, Y, Mz, Mx, My, Z, q
    u0 = np.array(u0)
    u1 = np.array(u1)

    # 交换顺序
    # X, Y, Mz, Mx, My, Z, q
    # Y, Mz, Mx, X, Z, My
    def change_order(ua):
        ub = np.zeros(6)
        ub[0] = ua[2]-ua[1] # Y
        ub[1] = ua[1]+ua[2] # Mz
        ub[2] = ua[3]   # Mx
        ub[3] = ua[0]   # X
        ub[4] = ua[5] - ua[4]  # Z
        ub[5] = ua[4] + ua[5]   # My
        return ub

    # 动压电压
    uq0 = u0[-1]
    uq1 = u1[-1]


    # 气动力电压
    # Y, Mz, Mx, X, Z, My
    uf0 = change_order(u0)
    uf1 = change_order(u1)
    ufd = uf1-uf0

    # 计算动压
    q = (uq1-uq0)/24.26097143*9.80665

    # 计算初始气动力
    f0 = np.ones(6)
    f = np.ones(6)

    for i in range(6):
        f0[i] = ufd[i]*matrix_steady[i,i]
        
    # 气动力迭代
    # Y, Mz, Mx, X, Z, My
    def iter_force(f1,f0):
        X = f1[3]
        Y = f1[0]
        Z = f1[4]
        Mx = f1[2]
        My = f1[5]
        Mz = f1[1]

        f2 = np.zeros(6)

        item = np.array([Y, Mz, Mx, X, Z, My,
                        Y*Y, Mz*Mz, Mx*Mx, X*X, Z*Z, My*My,
                        Y*Mz, Y*Mx, Y*X, Y*Z, Y*My, 
                        Mz*Mx, Mz*X, Mz*Z, Mz*My, 
                        Mx*X, Mx*Z, Mx*My, 
                        X*Z, X*My, 
                        Z*My])

        for i in range(6):
            coeff = matrix_steady[:,i]
            coeff[i] = 0
            f2[i] = f0[i] + np.sum(np.multiply(item, coeff))

        return f2

    if iter_num == 0:
        f = f0
    else:
        for i in range(iter_num):
            f = iter_force(f,f0)

    # Y, Mz, Mx, X, Z, My
    # X, Y, Z, Mx, My, Mz

    ft = np.zeros(6)
    ft[0] = f[3]
    ft[1] = f[0]
    ft[2] = f[4]
    ft[3] = f[2]
    ft[4] = f[5]
    ft[5] = f[1]
        
    # print(q)
    return ft,q

'''
计算气动力系数
C = F/(QS)
'''
def calc_coeff(F_n_Q, model):
    F_n_Q = np.array(F_n_Q)

    if(model == "F22_STATIC" or model == "F22_10" or model=="22_SWEEP"):
        model = "F22"

    #  机翼参考面积
    area_dict = {"F22":0.1064217,"F16":4.475e-2}
    S = area_dict[model]

    # 平均气动弦长
    chord_dict = {"F22":0.2522979,"F16":4.475e-2}
    c = chord_dict[model]

    # 机翼展长
    span_dict = {"F22":0.5008725,"F16":4.475e-2}
    b = span_dict[model]


    Q = F_n_Q[:,-1]
    Q = np.abs(np.average(Q))

    C_n_Q = F_n_Q/(Q*S)

    C_n_Q[:,3] = C_n_Q[:,3]/b # 滚转力矩
    C_n_Q[:,4] = C_n_Q[:,4]/c # 俯仰力矩
    C_n_Q[:,5] = C_n_Q[:,5]/b # 偏航力矩

    return C_n_Q





    
